//
// Created by billchent on 2020/9/23.
//

#ifndef SDUOG_CONTROLLER_FSM_STATE_RL_LOCOMOTION_H
#define SDUOG_CONTROLLER_FSM_STATE_RL_LOCOMOTION_H

#include "FSM_State.h"
#include "../Controllers/RL_Controller/include/rl_controllers/LearningBasedController.h"

/**
 *
 */

template <typename T>
class FSM_State_RL_Locomotion : public FSM_State<T>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    FSM_State_RL_Locomotion(ControlFSMData<T> *_controlFSMData);

    // Behavior to be carried out when entering a state
    void onEnter();

    // Run the normal behavior for the state
    void run();

    // Checks for any transition triggers
    FSM_StateName checkTransition();

    // Manages state specific transitions
    TransitionData<T> transition();

    // Behavior to be carried out when exiting a state
    void onExit();

private:
    // Keep track of the control iterations
    int iter = 0;
    LearningBasedController *rlActualController;

    // Parses contact specific controls to the leg controller
    void LocomotionControlStep();

    bool locomotionSafe();
};
#endif // SDUOG_CONTROLLER_FSM_STATE_RL_LOCOMOTION_H
